#include "camera.h"

int currentCamera = 0;
int nCameras = 0;

extern int glWidth, glHeight;

static int camera_cb(p_ply_argument argument)
{
  long elem; double val;
  void *data; VCGCamera *cam;

  ply_get_argument_user_data(argument, &data, &elem);
  cam = (VCGCamera *) data;
  val = ply_get_argument_value(argument);

  switch (elem)
  {
  case 0: cam->vp[X] = val; break;
  case 1: cam->vp[Y] = val; break;
  case 2: cam->vp[Z] = val; break;
  case 3: cam->xa[X] = val; break;
  case 4: cam->xa[Y] = val; break;
  case 5: cam->xa[Z] = val; break;
  case 6: cam->ya[X] = val; break;
  case 7: cam->ya[Y] = val; break;
  case 8: cam->ya[Z] = val; break;
  case 9: cam->za[X] = val; break;
  case 10: cam->za[Y] = val; break;
  case 11: cam->za[Z] = val; break;
  case 12: cam->focal = val; break;
  case 13: cam->sx = val; break;
  case 14: cam->sy = val; break;
  case 15: cam->cx = val; break;
  case 16: cam->cy = val; break;
  case 17: cam->vx = val; break;
  case 18: cam->vy = val; break;
  case 19: cam->k1 = val; break;
  case 20: cam->k2 = val; break;
  case 21: cam->k3 = val; break;
  case 22: cam->k4 = val; break;
  case 23: cam->n = val; break;
  case 24: cam->f = val; break;
  }
  return 1;
}

VCGCamera *read_ply_cam(char *fname, VCGCamera *cam)
{
  int nc; const char *value;
  p_ply ply = ply_open(fname, NULL);

  if (!ply) return NULL;

  if (!ply_read_header(ply)) return NULL;

  nc = ply_set_read_cb(ply, "camera", "view_px", camera_cb, cam, 0);
  ply_set_read_cb(ply, "camera", "view_py", camera_cb, cam, 1);
  ply_set_read_cb(ply, "camera", "view_pz", camera_cb, cam, 2);

  ply_set_read_cb(ply, "camera", "x_axisx", camera_cb, cam, 3);
  ply_set_read_cb(ply, "camera", "x_axisy", camera_cb, cam, 4);
  ply_set_read_cb(ply, "camera", "x_axisz", camera_cb, cam, 5);

  ply_set_read_cb(ply, "camera", "y_axisx", camera_cb, cam, 6);
  ply_set_read_cb(ply, "camera", "y_axisy", camera_cb, cam, 7);
  ply_set_read_cb(ply, "camera", "y_axisz", camera_cb, cam, 8);

  ply_set_read_cb(ply, "camera", "z_axisx", camera_cb, cam, 9);
  ply_set_read_cb(ply, "camera", "z_axisy", camera_cb, cam, 10);
  ply_set_read_cb(ply, "camera", "z_axisz", camera_cb, cam, 11);

  ply_set_read_cb(ply, "camera", "focal", camera_cb, cam, 12);

  ply_set_read_cb(ply, "camera", "scalex", camera_cb, cam, 13);
  ply_set_read_cb(ply, "camera", "scaley", camera_cb, cam, 14);

  ply_set_read_cb(ply, "camera", "centerx", camera_cb, cam, 15);
  ply_set_read_cb(ply, "camera", "centery", camera_cb, cam, 16);

  ply_set_read_cb(ply, "camera", "viewportx", camera_cb, cam, 17);
  ply_set_read_cb(ply, "camera", "viewporty", camera_cb, cam, 18);

  ply_set_read_cb(ply, "camera", "k1", camera_cb, cam, 19);
  ply_set_read_cb(ply, "camera", "k2", camera_cb, cam, 20);
  ply_set_read_cb(ply, "camera", "k3", camera_cb, cam, 21);
  ply_set_read_cb(ply, "camera", "k4", camera_cb, cam, 22);
  
  ply_set_read_cb(ply, "camera", "n", camera_cb, cam, 23);
  ply_set_read_cb(ply, "camera", "f", camera_cb, cam, 24);

  if (!ply_read(ply)) return NULL;

  ply_close(ply);
  return cam;
}


bool write_ply_cam(char *fname, VCGCamera *cam)
{
  p_ply ply = ply_create(fname, PLY_ASCII, NULL);
  if (!ply) return false;
  if (!ply_add_comment(ply, "VCGLIB generated")) return false;

  if (!ply_add_element(ply, "camera", 1)) return false;

  if (!ply_add_property(ply, "view_px", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "view_py", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "view_pz", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_add_property(ply, "x_axisx", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "x_axisy", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "x_axisz", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_add_property(ply, "y_axisx", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "y_axisy", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "y_axisz", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_add_property(ply, "z_axisx", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "z_axisy", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "z_axisz", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_add_property(ply, "focal", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_add_property(ply, "scalex", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "scaley", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_add_property(ply, "centerx", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "centery", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_add_property(ply, "viewportx", PLY_INT, PLY_INT, PLY_INT)) return false;
  if (!ply_add_property(ply, "viewporty", PLY_INT, PLY_INT, PLY_INT)) return false;

  if (!ply_add_property(ply, "k1", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "k2", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "k3", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "k4", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_add_property(ply, "n", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;
  if (!ply_add_property(ply, "f", PLY_FLOAT, PLY_INT, PLY_FLOAT)) return false;

  if (!ply_write_header(ply)) return false;

  ply_write(ply, cam->vp[X]);
  ply_write(ply, cam->vp[Y]);
  ply_write(ply, cam->vp[Z]);
  ply_write(ply, cam->xa[X]);
  ply_write(ply, cam->xa[Y]);
  ply_write(ply, cam->xa[Z]);
  ply_write(ply, cam->ya[X]);
  ply_write(ply, cam->ya[Y]);
  ply_write(ply, cam->ya[Z]);
  ply_write(ply, cam->za[X]);
  ply_write(ply, cam->za[Y]);
  ply_write(ply, cam->za[Z]);
  ply_write(ply, cam->focal);
  ply_write(ply, cam->sx);
  ply_write(ply, cam->sy);
  ply_write(ply, cam->cx);
  ply_write(ply, cam->cy);
  ply_write(ply, cam->vx);
  ply_write(ply, cam->vy);
  ply_write(ply, cam->k1);
  ply_write(ply, cam->k2);
  ply_write(ply, cam->k3);
  ply_write(ply, cam->k4);
  ply_write(ply, cam->n);
  ply_write(ply, cam->f);

  if (!ply_close(ply)) return false;
}


int read_calibration(char *filename,double *Kc, double *Rc,
                     double *Tc,double *Kp, double *Rp,double *Tp,
                     double *Rcp,double *Tcp, double *Icp)
{
  FILE *arq;

  if((arq=fopen(filename,"r"))==NULL) return 0;

  fscanf(arq,"CAMERA CALIBRATION\n\n");

  fscanf(arq,"Kc: \n");
  fscanf(arq,"%lf %lf %lf \n",&Kc[0],&Kc[1],&Kc[2]);
  fscanf(arq,"%lf %lf %lf \n",&Kc[3],&Kc[4],&Kc[5]);
  fscanf(arq,"%lf %lf %le \n\n",&Kc[6],&Kc[7],&Kc[8]);

  fscanf(arq,"Rc: \n");
  fscanf(arq,"%lf %lf %lf \n",&Rc[0],&Rc[1],&Rc[2]);
  fscanf(arq,"%lf %lf %lf \n",&Rc[3],&Rc[4],&Rc[5]);
  fscanf(arq,"%lf %lf %lf \n\n",&Rc[6],&Rc[7],&Rc[8]);

  fscanf(arq,"Tc: \n");
  fscanf(arq,"%lf %lf %lf \n",&Tc[0],&Tc[1],&Tc[2]);

  fscanf(arq,"\nPROJECTOR CALIBRATION\n\n");

  fscanf(arq,"Kp: \n");
  fscanf(arq,"%lf %lf %lf \n",&Kp[0],&Kp[1],&Kp[2]);
  fscanf(arq,"%lf %lf %lf \n",&Kp[3],&Kp[4],&Kp[5]);
  fscanf(arq,"%lf %le %lf \n\n",&Kp[6],&Kp[7],&Kp[8]);

  fscanf(arq,"Rp: \n");
  fscanf(arq,"%lf %lf %lf \n",&Rp[0],&Rp[1],&Rp[2]);
  fscanf(arq,"%lf %lf %lf \n",&Rp[3],&Rp[4],&Rp[5]);
  fscanf(arq,"%lf %lf %lf \n\n",&Rp[6],&Rp[7],&Rp[8]);

  fscanf(arq,"Tp: \n");
  fscanf(arq,"%lf %lf %lf \n",&Tp[0],&Tp[1],&Tp[2]);

  fscanf(arq,"\nCAMERA -> PROJECTOR CORRESPONDENCE\n\n");

  fscanf(arq,"Rcp: \n");
  fscanf(arq,"%lf %lf %lf \n",&Rcp[0],&Rcp[1],&Rcp[2]);
  fscanf(arq,"%lf %lf %lf \n",&Rcp[3],&Rcp[4],&Rcp[5]);
  fscanf(arq,"%lf %lf %lf \n\n",&Rcp[6],&Rcp[7],&Rcp[8]);

  fscanf(arq,"Tcp: \n");
  fscanf(arq,"%lf %lf %lf \n",&Tcp[0],&Tcp[1],&Tcp[2]);

  fscanf(arq,"Icp: \n");
  fscanf(arq,"%lf %lf \n",&Icp[0],&Icp[1]);
  fscanf(arq,"%lf %lf \n",&Icp[2],&Icp[3]);
  fclose(arq);
  return 1;
}


int read_xform(char *filename, double *T)
{
  FILE *arq;

  if((arq=fopen(filename,"r"))==NULL) return 0;

  fscanf(arq,"%lf %lf %lf %lf\n",&T[0],&T[1],&T[2],&T[3]);
  fscanf(arq,"%lf %lf %lf %lf\n",&T[4],&T[5],&T[6],&T[7]);
  fscanf(arq,"%lf %lf %lf %lf\n",&T[8],&T[9],&T[10],&T[11]);
  fscanf(arq,"%lf %lf %lf %lf\n",&T[12],&T[13],&T[14],&T[15]);

  fclose(arq);
  return 1;
}
